#include <Arduino.h>
#include <unity.h>
#include "test_calcs_common.h"
#include "schedule_calcs.h"
#include "crankMaths.h"
#include "../test_utils.h"

#define _countof(x) (sizeof(x) / sizeof (x[0]))

// void printFreeRam()
// {
//     char msg[128];
//     sprintf(msg, "freeRam: %u", freeRam());
//     TEST_MESSAGE(msg);
// }

struct inj_test_parameters
{
    uint16_t channelAngle;  // deg
    uint16_t pw;            // uS
    uint16_t crankAngle;    // deg
    uint32_t pending;       // Expected delay when channel status is PENDING
    uint32_t running;       // Expected delay when channel status is RUNNING
};

static void nullInjCallback(void) { }

static void test_calc_inj_timeout(const inj_test_parameters &parameters)
{
    static constexpr uint16_t injAngle = 355;
    char msg[150];
    uint16_t PWdivTimerPerDegree = timeToAngleDegPerMicroSec(parameters.pw);

    FuelSchedule schedule(FUEL2_COUNTER, FUEL2_COMPARE, nullInjCallback, nullInjCallback);
    uint16_t startAngle = 0;
    /*
    //Pending schedules are no longer tested as will always return 0;
    schedule.Status = PENDING;
    startAngle = calculateInjectorStartAngle(PWdivTimerPerDegree, parameters.channelAngle, injAngle);
    sprintf_P(msg, PSTR("PENDING channelAngle: %" PRIu16 ", pw: %" PRIu16 ", crankAngle: %" PRIu16 ", openAngle: %" PRIu16), parameters.channelAngle, parameters.pw, parameters.crankAngle, startAngle);
    TEST_ASSERT_INT32_WITHIN_MESSAGE(1, parameters.pending, calculateInjectorTimeout(schedule, startAngle, parameters.crankAngle), msg);
    */
    
    schedule.Status = RUNNING;
    startAngle = calculateInjectorStartAngle( PWdivTimerPerDegree, parameters.channelAngle, injAngle);
    sprintf_P(msg, PSTR("RUNNING channelAngle: %" PRIu16 ", pw: %" PRIu16 ", crankAngle: %" PRIu16 ", openAngle: %" PRIu16), parameters.channelAngle, parameters.pw, parameters.crankAngle, startAngle);
    TEST_ASSERT_INT32_WITHIN_MESSAGE(1, parameters.running, calculateInjectorTimeout(schedule, startAngle, parameters.crankAngle), msg);
}


static void test_calc_inj_timeout(const inj_test_parameters *pStart, const inj_test_parameters *pEnd)
{
    inj_test_parameters local;
    while (pStart!=pEnd)
    {
        memcpy_P(&local, pStart, sizeof(local));
        test_calc_inj_timeout(local);
        ++pStart;
    }
}

static void test_calc_inj_timeout_360()
{
  setEngineSpeed(4000, 360);

  // Expected test values were generated using floating point calculations (in Excel)
  
  static const inj_test_parameters test_data[] PROGMEM = {
    // ChannelAngle (deg), PW (uS), Crank (deg), Expected Pending (uS), Expected Running (uS)
    { 0                  , 3000   , 0          , 11792                , 11792 },
    { 0                  , 3000   , 45         , 9917                 , 9917 },
    { 0                  , 3000   , 90         , 8042                 , 8042 },
    { 0                  , 3000   , 135        , 6167                 , 6167 },
    { 0                  , 3000   , 180        , 4292                 , 4292 },
    { 0                  , 3000   , 215        , 2833                 , 2833 },
    { 0                  , 3000   , 270        , 542                  , 542 },
    { 0                  , 3000   , 315        , 0                    , 13667 },
    { 0                  , 3000   , 360        , 0                    , 11792 },
    { 72                 , 3000   , 0          , 14792                , 14792 },
    { 72                 , 3000   , 45         , 12917                , 12917 },
    { 72                 , 3000   , 90         , 11042                , 11042 },
    { 72                 , 3000   , 135        , 9167                 , 9167 },
    { 72                 , 3000   , 180        , 7292                 , 7292 },
    { 72                 , 3000   , 215        , 5833                 , 5833 },
    { 72                 , 3000   , 270        , 3542                 , 3542 },
    { 72                 , 3000   , 315        , 1667                 , 1667 },
    { 72                 , 3000   , 360        , 0                    , 14792 },
    { 80                 , 3000   , 0          , 125                  , 125 },
    { 80                 , 3000   , 45         , 0                    , 13250 },
    { 80                 , 3000   , 90         , 0                    , 11375 },
    { 80                 , 3000   , 135        , 0                    , 9500 },
    { 80                 , 3000   , 180        , 0                    , 7625 },
    { 80                 , 3000   , 215        , 6167                 , 6167 },
    { 80                 , 3000   , 270        , 3875                 , 3875 },
    { 80                 , 3000   , 315        , 2000                 , 2000 },
    { 80                 , 3000   , 360        , 125                  , 125 },
    { 90                 , 3000   , 0          , 542                  , 542 },
    { 90                 , 3000   , 45         , 0                    , 13667 },
    { 90                 , 3000   , 90         , 11792                , 11792 },
    { 90                 , 3000   , 135        , 9917                 , 9917 },
    { 90                 , 3000   , 180        , 8042                 , 8042 },
    { 90                 , 3000   , 215        , 6583                 , 6583 },
    { 90                 , 3000   , 270        , 4292                 , 4292 },
    { 90                 , 3000   , 315        , 2417                 , 2417 },
    { 90                 , 3000   , 360        , 542                  , 542 },
    { 144                , 3000   , 0          , 2792                 , 2792 },
    { 144                , 3000   , 45         , 917                  , 917 },
    { 144                , 3000   , 90         , 0                    , 14042 },
    { 144                , 3000   , 135        , 0                    , 12167 },
    { 144                , 3000   , 180        , 10292                , 10292 },
    { 144                , 3000   , 215        , 8833                 , 8833 },
    { 144                , 3000   , 270        , 6542                 , 6542 },
    { 144                , 3000   , 315        , 4667                 , 4667 },
    { 144                , 3000   , 360        , 2792                 , 2792 },
    { 180                , 3000   , 0          , 4292                 , 4292 },
    { 180                , 3000   , 45         , 2417                 , 2417 },
    { 180                , 3000   , 90         , 542                  , 542 },
    { 180                , 3000   , 135        , 0                    , 13667 },
    { 180                , 3000   , 180        , 11792                , 11792 },
    { 180                , 3000   , 215        , 10333                , 10333 },
    { 180                , 3000   , 270        , 8042                 , 8042 },
    { 180                , 3000   , 315        , 6167                 , 6167 },
    { 180                , 3000   , 360        , 4292                 , 4292 },
    { 240                , 3000   , 0          , 6792                 , 6792 },
    { 240                , 3000   , 45         , 4917                 , 4917 },
    { 240                , 3000   , 90         , 3042                 , 3042 },
    { 240                , 3000   , 135        , 1167                 , 1167 },
    { 240                , 3000   , 180        , 0                    , 14292 },
    { 240                , 3000   , 215        , 0                    , 12833 },
    { 240                , 3000   , 270        , 10542                , 10542 },
    { 240                , 3000   , 315        , 8667                 , 8667 },
    { 240                , 3000   , 360        , 6792                 , 6792 },
    { 270                , 3000   , 0          , 8042                 , 8042 },
    { 270                , 3000   , 45         , 6167                 , 6167 },
    { 270                , 3000   , 90         , 4292                 , 4292 },
    { 270                , 3000   , 135        , 2417                 , 2417 },
    { 270                , 3000   , 180        , 542                  , 542 },
    { 270                , 3000   , 215        , 0                    , 14083 },
    { 270                , 3000   , 270        , 11792                , 11792 },
    { 270                , 3000   , 315        , 9917                 , 9917 },
    { 270                , 3000   , 360        , 8042                 , 8042 },
    { 360                , 3000   , 0          , 11792                , 11792 },
    { 360                , 3000   , 45         , 9917                 , 9917 },
    { 360                , 3000   , 90         , 8042                 , 8042 },
    { 360                , 3000   , 135        , 6167                 , 6167 },
    { 360                , 3000   , 180        , 4292                 , 4292 },
    { 360                , 3000   , 215        , 2833                 , 2833 },
    { 360                , 3000   , 270        , 542                  , 542 },
    { 360                , 3000   , 315        , 0                    , 13667 },
    { 360                , 3000   , 360        , 11792                , 11792 },
  };

  test_calc_inj_timeout(&test_data[0], &test_data[0]+_countof(test_data));
}

static void test_calc_inj_timeout_720()
{
  setEngineSpeed(4000, 720);
  
  // Expected test values were generated using floating point calculations (in Excel)
  static const inj_test_parameters test_data[] PROGMEM = {
    // ChannelAngle (deg), PW (uS), Crank (deg), Expected Pending (uS), Expected Running (uS)
    { 0                  , 3000   , 0          , 11792                , 11792 },
    { 0                  , 3000   , 45         , 9917                 , 9917 },
    { 0                  , 3000   , 90         , 8042                 , 8042 },
    { 0                  , 3000   , 135        , 6167                 , 6167 },
    { 0                  , 3000   , 180        , 4292                 , 4292 },
    { 0                  , 3000   , 215        , 2833                 , 2833 },
    { 0                  , 3000   , 270        , 542                  , 542 },
    { 0                  , 3000   , 315        , 0                    , 28667 },
    { 0                  , 3000   , 360        , 0                    , 26792 },
    { 72                 , 3000   , 0          , 14792                , 14792 },
    { 72                 , 3000   , 45         , 12917                , 12917 },
    { 72                 , 3000   , 90         , 11042                , 11042 },
    { 72                 , 3000   , 135        , 9167                 , 9167 },
    { 72                 , 3000   , 180        , 7292                 , 7292 },
    { 72                 , 3000   , 215        , 5833                 , 5833 },
    { 72                 , 3000   , 270        , 3542                 , 3542 },
    { 72                 , 3000   , 315        , 1667                 , 1667 },
    { 72                 , 3000   , 360        , 0                    , 29792 },
    { 80                 , 3000   , 0          , 15125                , 15125 },
    { 80                 , 3000   , 45         , 13250                , 13250 },
    { 80                 , 3000   , 90         , 11375                , 11375 },
    { 80                 , 3000   , 135        , 9500                 , 9500 },
    { 80                 , 3000   , 180        , 7625                 , 7625 },
    { 80                 , 3000   , 215        , 6167                 , 6167 },
    { 80                 , 3000   , 270        , 3875                 , 3875 },
    { 80                 , 3000   , 315        , 2000                 , 2000 },
    { 80                 , 3000   , 360        , 125                  , 125 },
    { 90                 , 3000   , 0          , 15542                , 15542 },
    { 90                 , 3000   , 45         , 0                    , 13667 },
    { 90                 , 3000   , 90         , 11792                , 11792 },
    { 90                 , 3000   , 135        , 9917                 , 9917 },
    { 90                 , 3000   , 180        , 8042                 , 8042 },
    { 90                 , 3000   , 215        , 6583                 , 6583 },
    { 90                 , 3000   , 270        , 4292                 , 4292 },
    { 90                 , 3000   , 315        , 2417                 , 2417 },
    { 90                 , 3000   , 360        , 542                  , 542 },
    { 144                , 3000   , 0          , 0                    , 17792 },
    { 144                , 3000   , 45         , 0                    , 15917 },
    { 144                , 3000   , 90         , 0                    , 14042 },
    { 144                , 3000   , 135        , 0                    , 12167 },
    { 144                , 3000   , 180        , 10292                , 10292 },
    { 144                , 3000   , 215        , 8833                 , 8833 },
    { 144                , 3000   , 270        , 6542                 , 6542 },
    { 144                , 3000   , 315        , 4667                 , 4667 },
    { 144                , 3000   , 360        , 2792                 , 2792 },
    { 180                , 3000   , 0          , 0                    , 19292 },
    { 180                , 3000   , 45         , 0                    , 17417 },
    { 180                , 3000   , 90         , 0                    , 15542 },
    { 180                , 3000   , 135        , 0                    , 13667 },
    { 180                , 3000   , 180        , 11792                , 11792 },
    { 180                , 3000   , 215        , 10333                , 10333 },
    { 180                , 3000   , 270        , 8042                 , 8042 },
    { 180                , 3000   , 315        , 6167                 , 6167 },
    { 180                , 3000   , 360        , 4292                 , 4292 },
    { 240                , 3000   , 0          , 0                    , 21792 },
    { 240                , 3000   , 45         , 0                    , 19917 },
    { 240                , 3000   , 90         , 0                    , 18042 },
    { 240                , 3000   , 135        , 0                    , 16167 },
    { 240                , 3000   , 180        , 0                    , 14292 },
    { 240                , 3000   , 215        , 0                    , 12833 },
    { 240                , 3000   , 270        , 10542                , 10542 },
    { 240                , 3000   , 315        , 8667                 , 8667 },
    { 240                , 3000   , 360        , 6792                 , 6792 },
    { 270                , 3000   , 0          , 0                    , 23042 },
    { 270                , 3000   , 45         , 0                    , 21167 },
    { 270                , 3000   , 90         , 0                    , 19292 },
    { 270                , 3000   , 135        , 0                    , 17417 },
    { 270                , 3000   , 180        , 0                    , 15542 },
    { 270                , 3000   , 215        , 0                    , 14083 },
    { 270                , 3000   , 270        , 11792                , 11792 },
    { 270                , 3000   , 315        , 9917                 , 9917 },
    { 270                , 3000   , 360        , 8042                 , 8042 },
    { 360                , 3000   , 0          , 0                    , 26792 },
    { 360                , 3000   , 45         , 0                    , 24917 },
    { 360                , 3000   , 90         , 0                    , 23042 },
    { 360                , 3000   , 135        , 0                    , 21167 },
    { 360                , 3000   , 180        , 0                    , 19292 },
    { 360                , 3000   , 215        , 0                    , 17833 },
    { 360                , 3000   , 270        , 0                    , 15542 },
    { 360                , 3000   , 315        , 0                    , 13667 },
    { 360                , 3000   , 360        , 11792                , 11792 },
    { 480                , 3000   , 0          , 1792                 , 1792 },
    { 480                , 3000   , 45         , 0                    , 29917 },
    { 480                , 3000   , 90         , 0                    , 28042 },
    { 480                , 3000   , 135        , 0                    , 26167 },
    { 480                , 3000   , 180        , 0                    , 24292 },
    { 480                , 3000   , 215        , 0                    , 22833 },
    { 480                , 3000   , 270        , 0                    , 20542 },
    { 480                , 3000   , 315        , 0                    , 18667 },
    { 480                , 3000   , 360        , 0                    , 16792 },
    { 540                , 3000   , 0          , 4292                 , 4292 },
    { 540                , 3000   , 45         , 2417                 , 2417 },
    { 540                , 3000   , 90         , 542                  , 542 },
    { 540                , 3000   , 135        , 0                    , 28667 },
    { 540                , 3000   , 180        , 0                    , 26792 },
    { 540                , 3000   , 215        , 0                    , 25333 },
    { 540                , 3000   , 270        , 0                    , 23042 },
    { 540                , 3000   , 315        , 0                    , 21167 },
    { 540                , 3000   , 360        , 0                    , 19292 },
    { 600                , 3000   , 0          , 6792                 , 6792 },
    { 600                , 3000   , 45         , 4917                 , 4917 },
    { 600                , 3000   , 90         , 3042                 , 3042 },
    { 600                , 3000   , 135        , 1167                 , 1167 },
    { 600                , 3000   , 180        , 0                    , 29292 },
    { 600                , 3000   , 215        , 0                    , 27833 },
    { 600                , 3000   , 270        , 0                    , 25542 },
    { 600                , 3000   , 315        , 0                    , 23667 },
    { 600                , 3000   , 360        , 0                    , 21792 },
    { 630                , 3000   , 0          , 8042                 , 8042 },
    { 630                , 3000   , 45         , 6167                 , 6167 },
    { 630                , 3000   , 90         , 4292                 , 4292 },
    { 630                , 3000   , 135        , 2417                 , 2417 },
    { 630                , 3000   , 180        , 542                  , 542 },
    { 630                , 3000   , 215        , 0                    , 29083 },
    { 630                , 3000   , 270        , 0                    , 26792 },
    { 630                , 3000   , 315        , 0                    , 24917 },
    { 630                , 3000   , 360        , 0                    , 23042 },
  };

  test_calc_inj_timeout(&test_data[0], &test_data[0]+_countof(test_data));
}

// 
void test_calc_inj_timeout(void)
{
  SET_UNITY_FILENAME() {

    RUN_TEST(test_calc_inj_timeout_360);
    RUN_TEST(test_calc_inj_timeout_720);
  }
}